Model Evaluation

Source: vignettes/model_evaluation.Rmd

First of all, we need to load the reference trajectory and create a trajectory object out of it (see ?make_trajectory for details).

data("example_2_traj_ned") 
head(example_2_traj_ned)
##      [,1]     [,2]     [,3]      [,4]     [,5]     [,6]      [,7]
## [1,] 0.00 324.1190 111.2354 -96.87579 0.078992 0.066954 -6.305269
## [2,] 0.01 324.2423 111.2518 -96.87778 0.078632 0.067613 -6.304354
## [3,] 0.02 324.3657 111.2683 -96.87877 0.077551 0.068031 -6.303471
## [4,] 0.03 324.4890 111.2849 -96.87977 0.076253 0.068448 -6.302570
## [5,] 0.04 324.6124 111.3015 -96.88176 0.074953 0.068832 -6.301709
## [6,] 0.05 324.7357 111.3182 -96.88375 0.073761 0.068896 -6.300924
traj = make_trajectory(data = example_2_traj_ned, system = "ned")

Let’s see how the reference trajectory looks like.

# plot traj
plot(traj)

plot(traj, threeD = T)

Then we need to make a timing object (see ?make_timing for details) where we specify

  • the start and the end of navigation,
  • the frequencies of different sensors,
  • the beginning and end of the GPS outage period.
timing = make_timing(nav.start     = 0, # time at which to begin filtering
                     nav.end       = 120, 
                     freq.imu      = 100, # frequency of the IMU, can be slower wrt trajectory frequency
                     freq.gps      = 1, # GNSS frequency
                     freq.baro     = 1, # barometer frequency (to disable, put it very low, e.g. 1e-5)
                     gps.out.start = 60, # to simulate a GNSS outage, set a time before nav.end
                     gps.out.end   = 100)

Now we need to create the sensor error models for error generation as a list of sensor objects (see ?make_sensor for details). These are the models that will be used in generating the sensor errors, and not the ones necessarily used within the navigation filter.

snsr.mdl=list()

# this uses a model for noise data generation
acc.mdl = WN(sigma2 = 5.989778e-05) + AR1(phi = 9.982454e-01, sigma2 = 1.848297e-10) + AR1(phi = 9.999121e-01, sigma2 = 2.435414e-11) + AR1(phi = 9.999998e-01, sigma2 = 1.026718e-12)
gyr.mdl = WN(sigma2 = 1.503793e-06) + AR1(phi = 9.968999e-01, sigma2 = 2.428980e-11) + AR1(phi = 9.999001e-01, sigma2 = 1.238142e-12)
snsr.mdl$imu = make_sensor(name="imu", frequency=timing$freq.imu, error_model1=acc.mdl, error_model2=gyr.mdl)

# RTK-like GNSS 
gps.mdl.pos.hor = WN(sigma2 = 0.025^2)
gps.mdl.pos.ver = WN(sigma2 = 0.05^2)
gps.mdl.vel.hor = WN(sigma2 = 0.01^2)
gps.mdl.vel.ver = WN(sigma2 = 0.02^2)
snsr.mdl$gps = make_sensor(name="gps", frequency=timing$freq.gps,
                           error_model1=gps.mdl.pos.hor,
                           error_model2=gps.mdl.pos.ver,
                           error_model3=gps.mdl.vel.hor,
                           error_model4=gps.mdl.vel.ver)

# Barometer
baro.mdl = WN(sigma2=0.5^2)
snsr.mdl$baro = make_sensor(name="baro", frequency=timing$freq.baro, error_model1=baro.mdl)

Then we need to create the sensor error models for filtering as a list of sensor objects (see ?make_sensor for details). These are the models that will be used within the navigation filter (an extended Kalman filter), which may or may not be the same as the ones used in generating the sensor errors. In this example, we have chosen them to be the same.

KF.mdl = list()

# make IMU sensor
KF.mdl$imu = make_sensor(name="imu", frequency=timing$freq.imu, error_model1=acc.mdl, error_model2=gyr.mdl)

KF.mdl$gps  = snsr.mdl$gps
KF.mdl$baro = snsr.mdl$baro

Finally, we can call the navigation function, which first simulates realistic sensor data based on the reference trajectory and provided sensor error models, and then performs the navigation. The whole process can be done in a Monte-Carlo fashion, by only specifying the number of desired runs as the num.runs input to navigation function. For detailed documentation, see ?navigation.

num.runs = 50 # number of Monte-Carlo simulations
res = navigation(traj.ref = traj,
                 timing = timing,
                 snsr.mdl = snsr.mdl,
                 KF.mdl = KF.mdl,
                 num.runs = num.runs,
                 noProgressBar = TRUE,
                 PhiQ_method = "4", # order of the taylor expansion of the matrix exponential used to compute Phi and Q matrices
                 compute_PhiQ_each_n = 10, # compute new Phi and Q matrices every n IMU steps (execution time optimization)
                 parallel.ncores=24,
                 P_subsampling = timing$freq.imu) # keep one covariance every second
## Monte-Carlo runs...

We can now see how the results look like.

plot(res, plot3d = T, time_interval = .5, emu_for_covmat = 1,
     emu_to_plot = 1, time_interval_simu = 1, error_analysis = F,
     plot_mean_traj = T, plot_CI = F, nsim = num.runs)